#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from mavros_msgs.msg import State

def state_publisher():
    rospy.init_node('simulated_state_publisher', anonymous=True)
    state_pub = rospy.Publisher('mavros/state', State, queue_size=10)
    
    rate = rospy.Rate(10)  # 10Hz
    
    # 创建State消息对象
    current_state = State()
    
    # 设置默认状态
    current_state.connected = True
    current_state.armed = False
    current_state.guided = False
    current_state.mode = "STABILIZE"
    current_state.system_status = 3  # STANDBY
    
    while not rospy.is_shutdown():
        # 模拟状态变化
        if rospy.get_time() > 10.0:  # 10秒后切换到OFFBOARD模式
            current_state.mode = "OFFBOARD"
        
        if rospy.get_time() > 15.0:  # 15秒后解锁
            current_state.armed = True
        
        # 发布状态消息
        state_pub.publish(current_state)
        
        rospy.loginfo("发布状态: 模式=%s, 解锁=%s", 
                     current_state.mode, 
                     "是" if current_state.armed else "否")
        
        rate.sleep()

if __name__ == '__main__':
    try:
        state_publisher()
    except rospy.ROSInterruptException:
        pass
